﻿using Microsoft.Xna.Framework;
using System;

namespace BlawkayEngine
{
    public static class VectorHelper
    {

        /// <summary>
        /// Returns the forward direction along the XZ Plane
        /// </summary>
        public static Vector3 FlatDirection(Vector3 vActualForward)
        {
            vActualForward.Y = 0;

            if (vActualForward != Vector3.Zero)
            {
                vActualForward.Normalize();
            }

            return vActualForward;
        }


        /// <summary> 
        /// The function converts a Microsoft.Xna.Framework.Quaternion into a Microsoft.Xna.Framework.Vector3 
        /// </summary> 
        /// <param name="q">The Quaternion to convert</param> 
        /// <returns>An equivalent Vector3</returns> 
        /// <remarks> 
        /// This function was extrapolated by reading the work of Martin John Baker. All credit for this function goes to Martin John. 
        /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm 
        /// </remarks> 
        public static Vector3 QuaternionToEuler(Quaternion q)
        {
            Vector3 v = new Vector3();

            v.X = (float)Math.Atan2(
                2 * q.Y * q.W - 2 * q.X * q.Z,
                1 - 2 * Math.Pow(q.Y, 2) - 2 * Math.Pow(q.Z, 2));

            v.Y = (float)Math.Asin(
                2 * q.X * q.Y + 2 * q.Z * q.W);

            v.Z = (float)Math.Atan2(
                2 * q.X * q.W - 2 * q.Y * q.Z,
                1 - 2 * Math.Pow(q.X, 2) - 2 * Math.Pow(q.Z, 2));

            if (q.X * q.Y + q.Z * q.W == 0.5)
            {
                v.X = (float)(2 * Math.Atan2(q.X, q.W));
                v.Z = 0;
            }

            else if (q.X * q.Y + q.Z * q.W == -0.5)
            {
                v.X = (float)(-2 * Math.Atan2(q.X, q.W));
                v.Z = 0;
            }

            return v;
        }

        public static int Round(float value, int factor)
        {
            int nearestMultiple = (int)Math.Round((double)(value / (float)factor)) * factor;
            return nearestMultiple;        
        }

        /// <summary>
        /// Changes a Vector into an Angle in Radians
        /// </summary>
        /// <param name="vector">Vector in some direction</param>
        /// <returns>The Angle in Radians</returns>
        public static float VectorToAngle(Vector2 vector)
        {
            return (float)Math.Atan2(vector.Y, vector.X);
        }

        public static readonly Vector3 GLOBAL_UP = Vector3.Up;
        public static readonly Vector3 GLOBAL_BACKWARD = Vector3.Backward;
        public static readonly Vector3 GLOBAL_RIGHT = Vector3.Right;

    }
}
